#include "socket.h"
#include <sstream>
#include <vector>
using namespace std;

#include <ros/ros.h>

Socket* serverSocket = NULL;
vector<Socket*> clients;

void SendIRB(Socket* sock, string str)
{
	sock->Send((uint8_t*)str.c_str(), str.length()+1);
}

bool RecvIRB(Socket* sock, string& str)
{
	uint8_t buff[1024];
	size_t len;
	bool ret = sock->Recv(buff,len,false);

	str = (char*)buff;
	return ret;
}

// Robot state
bool grip = false;
double x = 1, y = 2, z = 3;
double rx = 4, ry = 5, rz = 6, rw = 7;

void timerCallback(const ros::TimerEvent&)
{
	Socket* newSocket = serverSocket->Accept();

	if(newSocket)
		clients.push_back(newSocket);

	for(vector<Socket*>::iterator it = clients.begin(); it != clients.end(); ++it)
	{
		Socket* sock = *it;
		string str;

		bool gotData = RecvIRB(sock, str);
		if(gotData)
		{
			ROS_INFO("Received data: [%s]", str.c_str());
			if(str.find("<Get Gripper>") != string::npos)
			{
				stringstream ss;
				ss << "<" << (int)grip << ">";
				SendIRB(sock, ss.str());
			}
			else if(str.find("<Get Pose>") != string::npos)
			{
				stringstream ss;
				ss << "<[[" << x  << "," <<
					  y  << "," <<
					  z  << "],[" <<
					  rx << "," <<
					  ry << "," <<
					  rz << "," <<
					  rw << "]]>";
				SendIRB(sock, ss.str());
			}
			else if(str.find("<Set Gripper") != string::npos)
			{
				stringstream ss(str);
				ss.ignore(strlen("<Set Gripper"));
				int val;
				ss >> val;
				grip = val;
				SendIRB(sock, "<ok>");
			}
			else if(str.find("<Move Target [[") != string::npos)
			{
				stringstream ss(str);
				ss.ignore(strlen("<Move Target [["));
				ss >> x; ss.ignore();
				ss >> y; ss.ignore();
				ss >> z; ss.ignore(3);
				ss >> rx; ss.ignore();
				ss >> ry; ss.ignore();
				ss >> rz; ss.ignore();
				ss >> rw; ss.ignore();
				SendIRB(sock, "<ok>");
			}
			else
				SendIRB(sock, "<ok>");
		}
	}
}

int main(int argc, char **argv)
{
	int port = 0;

	// Start ROS
	ros::init(argc, argv, "irb_simulator");
	ros::NodeHandle n;

	// Read arguments <port>
	if(argc != 2)
	{
		ROS_ERROR("Missing port! Usage: %s <port>", argv[0]);
		return 1;
	}
	else
	{
		stringstream ss(argv[1]);
		ss >> port;
	}

	// Create a periodic callback
	ros::Timer timer = n.createTimer(ros::Duration(0.1), timerCallback);

	// Initialize a listening socket
	Socket::Init();
	serverSocket = new Socket(port);

	// Check if socket was initialized correctly
	if(!serverSocket || !serverSocket->Ok())
		return 1;

	// Enter main loop
	ros::spin();

	return 0;
}
